//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================
#include "xllp_defs.h"
#include "xllp_serialization.h"
#include "xllp_lcd.h"
#include "xllp_gpio.h"
#include "xllp_ost.h"
#include "_display.h"

#define SET_TX_DATA_H            p_GPIORegs->GPSR3  |= 0x00000001         // GPIO 96
#define SET_TX_DATA_L            p_GPIORegs->GPCR3  |= 0x00000001

#define GET_TX_OUT_DATA         (p_GPIORegs->GPLR3 & 0x00000001)

#define SET_SCL_H                p_GPIORegs->GPSR2  |= 0x80000000
#define SET_SCL_L                p_GPIORegs->GPCR2  |= 0x80000000

extern void D_Delay(unsigned int uSec);
extern void OSTDelay(int uSec);

int kprintf(const char *, ...);

typedef struct  LCD_PARA_STRU_T
{
	unsigned char     reg;
	unsigned char     data;
	int      OSTDelay;	     // us
}LCD_PARA_STRU;

LCD_PARA_STRU   toshibaParaList[] =
{
    {0x22,0x01,1000},
    {0x03,0x01,10000},
    {0x0,0xa0,0},
    {0x1,0x00,0},
    {0x2,0x0,0},
    {0x5,0x50,0},
    {0x0d,0x0,40000},
    {0x0e,0x0,0},
    {0x0F,0x0,0},
    {0x10,0x0,0},
    {0x11,0x0,0},
    {0x12,0x0,0},
    {0x13,0x0,0},
    {0x14,0x0,0},
    {0x15,0x0,0},
    {0x16,0x0,0},
    {0x17,0x0,0},
    {0x34,0x1,0},
    {0x35,0x1,30000},
    {0x8D,0x1,0},
    {0x2D,0x0,0},
    {0x2D,0x1,0},                    //Calibration Start
    {0x8B,0x27,0},

//Vertical scroll start
    {0x4B,0x0,0},
    {0x4C,0x0,0},
    {0x4D,0x0,0},
    {0x4E,0x0,0},
    {0x4F,0x0,0},
    {0x50,0x0,0},
// Vertical scroll stop
    {0x86,0x0,0},
    {0x87,0x23,0},
    {0x88,0x02,0},
    {0x89,0x03,0},
    {0x33,0x01,0},
    {0x37,0x06,0},
    {0x76,0x0,0},
    {0x42,0x0,0},
    {0x43,0x1,0},
    {0x44,0x3F,0},
    {0x45,0x0,0},
    {0x46,0xEF,0},
    {0x47,0x0,0},
    {0x48,0x0,0},
    {0x49,0x1,0},
    {0x4A,0x3F,0},

    {0x3C,0x0,0},
    {0x3D,0x0,0},
    {0x3E,0x1,0},
    {0x3F,0x3F,0},

    {0x40,0x03,0},
    {0x41,0x0A,0},
    {0x8F,0x20,30000},

    {0x90,0x3F,0},

    {0x91,0x47,0},
    {0x92,0x40,0},
    {0x93,0x44,0},
    {0x94,0x33,0},
    {0x95,0x20,0},
    {0x96,0x3F,0},
    {0x97,0x47,0},
    {0x98,0x44,0},
    {0x99,0x44,0},
    {0x9A,0x33,0},

    {0x9B,0x33,0},
    {0x9C,0x33,0},
    {0x9D,0x80,0},
    {0x1D,0x07,30000},
    {0x23,0x0,1000},
    {0x24,0x0,1000},
    {0x25,0x0,1000},

    {0x28,0x27,0},
    {0x1a,0x0,0},
    {0x21,0x10,0},
    {0x18,0x25,40000},
    {0x19,0xe8,0},
    {0x18,0xe5,10000},
    {0x18,0xf7,40000},
    {0x1b,0x0d,80000},

    {0x1f,0x90,0},
    {0x20,0x5c,0},
    {0x1e,0xc0,10000},
    {0x21,0x0,0},
    {0x3b,0x1,20000},
    {0x0,0x20,10000},

//RGB Internal Synchronous Capture

    {0x2,0x1,0},
    {0x9d,0x80,0},
    {0x37,0x1,0},
    {0x87,0x23,0},
    {0x40,0x3,0},
    {0x41,0xa,0}
};

LCD_PARA_STRU   toshibaLcdOff[] =
{
{0x00, 0x80, 25000},
{0x1e, 0x00, 2000},
{0x00, 0x88, 15000},
{0x18, 0x00, 2000},
{0x01, 0x11, 300000}
};

typedef enum
{
	WFS_ALL,
	WFS_ANY
} WFS_TYPE;

P_XLLP_OST_T p_xllp_OST;

XLLP_UINT32_T  XllpLock(XLLP_PROTECTED_REGISTER Xllp_RegisterID)
{
	return 0;
}

void XllpUnlock(XLLP_UINT32_T Xllp_LockID)
{
}

void  ssp_write_Register(XLLP_GPIO_T *p_GPIORegs,unsigned char reg,unsigned short data)
{
	unsigned long 	ucCnt,ucShift;
	unsigned long        ucData;

	p_GPIORegs->GPSR2  |=  XLLP_BIT_30;       // Set CS --> High       GPIO 94
	OSTDelay(4);
	p_GPIORegs->GPCR2  |=  XLLP_BIT_30;       // Set CS --> Low

	p_GPIORegs->GPCR3  |= XLLP_BIT_10;        // Set RSB --> LOw	GPIO 106

	ucData = reg;
	ucShift = 0x80;
	for (ucCnt=0; ucCnt<8; ucCnt++)
	{
		if (ucData&ucShift)
		{
			SET_TX_DATA_H;
		}
		else
		{
			SET_TX_DATA_L;
		}
		ucShift=ucShift>>1;
		OSTDelay(2);
		SET_SCL_L;
		OSTDelay(1);
		SET_SCL_H;
		OSTDelay(1);
	}

	p_GPIORegs->GPSR2  |=  XLLP_BIT_30;       // Set CS --> High       GPIO 94
	OSTDelay(4);
	p_GPIORegs->GPCR2  |=  XLLP_BIT_30;       // Set CS --> Low

	ucData = data;
	ucShift = 0x80;
	for (ucCnt=0;ucCnt<8;ucCnt++)
	{
		if (ucData&ucShift)
		{
			SET_TX_DATA_H;
		}
		else
		{
			SET_TX_DATA_L;
		}
		ucShift=ucShift>>1;
		OSTDelay(2);
		SET_SCL_L;
		OSTDelay(1);
		SET_SCL_H;
		OSTDelay(1);
	}

	p_GPIORegs->GPSR2  |=  XLLP_BIT_30;       // Set CS --> High       GPIO 94
//	p_GPIORegs->GPSR3  |= XLLP_BIT_10;        // Set RSB --> High	GPIO 106

	OSTDelay(4);
	p_GPIORegs->GPCR2  |=  XLLP_BIT_30;       // Set CS --> Low

}

XLLP_STATUS_T XllpLCDInit(P_XLLP_LCD_T pXllpLCD)
{
	XLLP_STATUS_T status = 0;
	LCDRegs *p_LCDRegs = (LCDRegs *) pXllpLCD->LCDC;

    // Before the controller, clear this bit
	p_LCDRegs->LCCR0 &= ~LCD_ENB;

	// Initialize the GPIO registers for proper LCD Controller operation
    LCDSetupGPIOs(pXllpLCD);
	// Initialize the LCD Controller and frame descriptors
	LCDInitController(pXllpLCD);

	// Clear LCD Controller status register
	LCDClearStatusReg(pXllpLCD);

	// Enable the LCD controller
	LCDEnableController(pXllpLCD);

	return status;
}

// Only overlay1 uses palette
void XllpLCDLoadPalette(P_XLLP_LCD_T pXllpLCD)
{
	LCDRegs *p_LCDRegs;

	p_LCDRegs = (LCDRegs *) pXllpLCD->LCDC;

	// Reconfigure the second frame descriptor so that when loaded,
	// this descriptor loops to itself.
	pXllpLCD->frameDescriptorCh1fd2->FDADR = LCD_FDADR(pXllpLCD->frameDescriptorCh1fd2->PHYSADDR);

	// Reconfigure the palette frame descriptor so that it loads the second frame descriptor
	pXllpLCD->frameDescriptorPalette->FDADR = LCD_FDADR(pXllpLCD->frameDescriptorCh1fd2->FDADR);
	pXllpLCD->frameDescriptorPalette->FSADR = LCD_FSADR(pXllpLCD->_PALETTE_BUFFER_BASE_PHYSICAL);
	pXllpLCD->frameDescriptorPalette->FIDR  = LCD_FIDR(0);

    // Overlays are enabled
    pXllpLCD->frameDescriptorPalette->LDCMD = LCD_Len(pXllpLCD->PaletteSize) | LCD_Pal;

	pXllpLCD->frameDescriptorPalette->PHYSADDR = LCD_FDADR(pXllpLCD->_PALETTE_FRAME_DESCRIPTOR_BASE_PHYSICAL);

	// Insert the palette descriptor into the descriptor chain to load the palette.
	// When this load completes, fd2 is automatically loaded next in the chain.
	// fd2 now loops to itself and continues to load frame data.
	pXllpLCD->frameDescriptorCh1->FDADR = LCD_FDADR(pXllpLCD->_PALETTE_FRAME_DESCRIPTOR_BASE_PHYSICAL);

	// swap frame descriptor pointers so that this operation is reversed the next time through
	pXllpLCD->frameDescriptorTemp	= pXllpLCD->frameDescriptorCh1;
	pXllpLCD->frameDescriptorCh1 = pXllpLCD->frameDescriptorCh1fd2;
	pXllpLCD->frameDescriptorCh1fd2 = pXllpLCD->frameDescriptorTemp;
}

void XllpLCDSuspend(P_XLLP_LCD_T pXllpLCD, int SuspendType)
{
	LCDRegs *p_LCDRegs;
	XLLP_GPIO_T *p_GPIORegs;

    p_LCDRegs = (LCDRegs *) pXllpLCD->LCDC;
	p_GPIORegs = (XLLP_GPIO_T *) pXllpLCD->GPIO;

	p_GPIORegs->GPCR2  |= XLLP_BIT_18;//GPIO82 LCD_PWRENB
	(void*)p_GPIORegs->GPCR2;
	SET_SCL_L;
	OSTDelay(5);

    if (p_LCDRegs->LCCR0 & LCD_ENB){
	    switch(SuspendType)	{
	        case Suspend_Graceful:
	    	    // Initiate power down sequence
	    	    p_LCDRegs->LCCR0 |= LCD_DIS;
	    	    // Wait for LDD bit to get set once the last DMA transfer has completed
	    	    OSTDelay(50*1000);
	    	    while(!(p_LCDRegs->LCSR0 & LCD_LDD)) {
	    		    kprintf("lcd deadloop in XllpLCDSuspend FDADR0 %x FSADR0 %x FIDR0 %x LDCMD0 %x \r\n",
	    				p_LCDRegs->FDADR0, p_LCDRegs->FSADR0, p_LCDRegs->FIDR0, p_LCDRegs->LDCMD0);
	    	    }

	    	    // Clear the sticky LDD bit
	    	    p_LCDRegs->LCSR0 |= LCD_LDD;
	    	    break;

	        case Suspend_Immediate:
	    	    p_LCDRegs->LCCR0 &= ~LCD_ENB;
	    	    break;

	        default :
	    	    break;
	    }
    }
	// don't use lock/unlock here because system call may be unavailable.
    p_GPIORegs->GPCR0	|= XLLP_GPIO_BIT_PWM_OUT0;
}

void XllpLCDResume(P_XLLP_LCD_T pXllpLCD)
{
	XLLP_GPIO_T *p_GPIORegs;
	p_GPIORegs = (XLLP_GPIO_T *) pXllpLCD->GPIO;

	XllpLCDInit(pXllpLCD);
	p_GPIORegs->GPSR2 |=  XLLP_BIT_18;                       // Digital Power 2.8v
//    SET_SCL_H;
//    OSTDelay(5);

}

void XllpLCDSetDisplayPage(P_XLLP_LCD_T pXllpLCD, int page)
{
	// Set the physical address of the frame buffer for all three frame descriptors
	// Make sure that you've initialized FrameBufferSize before calling this function either manually
	// or through a call to XllpLCDInit().
	pXllpLCD->CurrentPage = page;
	pXllpLCD->frameDescriptorCh0fd1->FSADR = LCD_FSADR(pXllpLCD->_FRAME_BUFFER_BASE_PHYSICAL + pXllpLCD->CurrentPage*pXllpLCD->FrameBufferSize);
	pXllpLCD->frameDescriptorCh1fd2->FSADR = LCD_FSADR(pXllpLCD->_FRAME_BUFFER_BASE_PHYSICAL + pXllpLCD->CurrentPage*pXllpLCD->FrameBufferSize);
	pXllpLCD->frameDescriptorCh1->FSADR = LCD_FSADR(pXllpLCD->_FRAME_BUFFER_BASE_PHYSICAL + pXllpLCD->CurrentPage*pXllpLCD->FrameBufferSize + (pXllpLCD->FrameBufferSize >> 1));
}


void  LCD_GPIO_Setup(XLLP_GPIO_T *p_GPIORegs)
{
	static const XLLP_UINT32_T lowpins[] = {6, 82, 94, 95, 96, 80, 19};
	static const XLLP_UINT32_T outpins[] = {6, 82, 94, 95, 96, 80, 19};
	static const XLLP_UINT32_T altpins[] = {6, 82, 94, 95, 96, 80, 19};
	static const XLLP_UINT32_T altfunc[] = {6, 0, 0, 0, 0, 0, 0};
	// configure processor pins
	//        Mux Pin         GPIO     Alt   Direction
	//        FLASH_ENB        11        0      O
	//        Digital PWB      82        0      O
	//        P_L_CS           94        0      O
	//        P_L_SCL          95        0      O
	//        P_L_SI           96        0      O
	//        P_RSB            80        0      O
	//        P_RST            19        0      O
	XllpGpioSetOutput0(p_GPIORegs, (XLLP_UINT32_T*)lowpins);
	XllpGpioSetDirectionOut(p_GPIORegs, (XLLP_UINT32_T*)outpins);
	XllpGpioSetAlternateFn (p_GPIORegs, (XLLP_UINT32_T*)altpins, (XLLP_UINT32_T*)altfunc);
}

void LCDOffToshibaQVGA(P_XLLP_LCD_T pXllpLCD)
{
	XLLP_GPIO_T *p_GPIORegs;
	int count, i;

	kprintf("lcd tt2 \r\n");
	p_GPIORegs = (XLLP_GPIO_T *) pXllpLCD->GPIO;

	count = sizeof(toshibaLcdOff)/sizeof(LCD_PARA_STRU);

	for(i=0; i<count; i++)
	{
		ssp_write_Register(p_GPIORegs, toshibaLcdOff[i].reg, toshibaLcdOff[i].data);
		if( toshibaLcdOff[i].OSTDelay > 0 )
		{
			OSTDelay(toshibaLcdOff[i].OSTDelay);
		}
	}

	p_GPIORegs->GPCR2  |= XLLP_BIT_18;                       // Digital Power 2.8v
	(void*)p_GPIORegs->GPCR2;
	//p_GPIORegs->GPCR3 |= XLLP_BIT_11;
	//(void *)p_GPIORegs->GPCR3;
}

void LCDInitToshibaQVGA(P_XLLP_LCD_T pXllpLCD)
{
	XLLP_GPIO_T *p_GPIORegs;
	int count, i;

	p_GPIORegs = (XLLP_GPIO_T *) pXllpLCD->GPIO;
	p_xllp_OST = (XLLP_OST_T *)  pXllpLCD->OST;
	p_GPIORegs = (XLLP_GPIO_T *) pXllpLCD->GPIO;

	//RGB_GPIO_Setup(p_GPIORegs);                // for test rgb led
	LCD_GPIO_Setup(p_GPIORegs);

	p_GPIORegs->GPSR2 |=  XLLP_BIT_18;                       // Digital Power 2.8v
	p_GPIORegs->GPCR0 |=  XLLP_BIT_19;                         // Reset pin
	p_GPIORegs->GPSR0 &= ~XLLP_BIT_19;
    OSTDelay(80000);

	p_GPIORegs->GPCR0 &= ~XLLP_BIT_19;                         // Reset pin
	p_GPIORegs->GPSR0 |= XLLP_BIT_19;
    OSTDelay(140000);

	// NReset  high
	count = sizeof(toshibaParaList)/sizeof(LCD_PARA_STRU);

	for(i=0; i<count; i++)
	{
		ssp_write_Register(p_GPIORegs,toshibaParaList[i].reg, toshibaParaList[i].data);
		if( toshibaParaList[i].OSTDelay > 0 )
		{
			OSTDelay(toshibaParaList[i].OSTDelay);
		}
	}

	//left to right
//	ssp_write_Register(p_GPIORegs, 0x1, 0xd0);
	p_GPIORegs->GPSR0 |= XLLP_GPIO_BIT_PWM_OUT0;
}

void LCDInitController(P_XLLP_LCD_T pXllpLCD)
{
	int BPP = 0;
	int PCD = 0;
	unsigned int CCCR_L = 0;
	LCDRegs *p_LCDRegs;
	XLLP_CLKMGR_T *p_CLKRegs;
	XLLP_SSPREGS_T *p_SSPRegs;
	XLLP_GPIO_T *p_GPIORegs;
	XLLP_OST_T *p_OSTRegs;
//	unsigned int StatMask = 0;

	int LCLK = 0;

	p_LCDRegs = (LCDRegs *) pXllpLCD->LCDC;
	p_CLKRegs = (XLLP_CLKMGR_T *) pXllpLCD->CLKMan;
	p_GPIORegs = (XLLP_GPIO_T *) pXllpLCD->GPIO;
	p_OSTRegs = (XLLP_OST_T *) pXllpLCD->OST;
	p_SSPRegs = (XLLP_SSPREGS_T *) pXllpLCD->SSP;

	p_LCDRegs->LCCR0 = 0;
	p_LCDRegs->LCCR1 = 0;
	p_LCDRegs->LCCR2 = 0;
	p_LCDRegs->LCCR3 = 0;
	p_LCDRegs->LCCR4 = 0;
	p_LCDRegs->LCCR5 = (LCD_SOFM1|LCD_SOFM2|LCD_SOFM3|LCD_SOFM4|LCD_SOFM5|LCD_SOFM6|
						LCD_EOFM1|LCD_EOFM2|LCD_EOFM3|LCD_EOFM4|LCD_EOFM5|LCD_EOFM6|
						LCD_BSM1 |LCD_BSM2 |LCD_BSM3 |LCD_BSM4 |LCD_BSM5 |LCD_BSM6 |
						LCD_IUM1 |LCD_IUM2 |LCD_IUM3 |LCD_IUM4 |LCD_IUM5 |LCD_IUM6 );

	// Determine the frame buffer size for the DMA transfer length.
	// Scale the size based on the bpp of the frame buffer to determine
	// an actual size in bytes
	pXllpLCD->FrameBufferSize = pXllpLCD->FrameBufferWidth * pXllpLCD->FrameBufferHeight;
	switch (pXllpLCD->BPP)
	{
		case BPP_1:
			pXllpLCD->FrameBufferSize >>= 3;
//			pXllpLCD->PaletteSize = 8;
			break;
		case BPP_2:
			pXllpLCD->FrameBufferSize >>= 2;
//			pXllpLCD->PaletteSize = 8;
			break;
		case BPP_4:
			pXllpLCD->FrameBufferSize >>= 1;
//			pXllpLCD->PaletteSize = 32;
			break;
		case BPP_8:
//			pXllpLCD->PaletteSize = 512;
			break;
		case BPP_16:
			pXllpLCD->FrameBufferSize <<= 1;
			break;
		case BPP_18_PACKED:
		case BPP_19_PACKED:
		    pXllpLCD->FrameBufferSize *= 3;
		    break;

		case BPP_18:		/* Fall through */
		case BPP_19:
		case BPP_24:
		case BPP_25:
			pXllpLCD->FrameBufferSize <<= 2;
			break;
		default:
			break;
	}

	// Enable the LCD
	p_CLKRegs->cken = (p_CLKRegs->cken & XLLP_CLKEN_MASK) | CLK_LCD ;

	// Configure the general purpose frame descriptors
	// Set the physical address of the frame descriptor
	pXllpLCD->frameDescriptorCh0fd1->FDADR = LCD_FDADR(pXllpLCD->_DMA_CHANNEL_0_FRAME_DESCRIPTOR_BASE_PHYSICAL);

	// Set the physical address of the frame buffer
	pXllpLCD->frameDescriptorCh0fd1->FSADR = LCD_FSADR(pXllpLCD->_FRAME_BUFFER_BASE_PHYSICAL + pXllpLCD->CurrentPage*pXllpLCD->FrameBufferSize);

	// Clear the frame ID
	pXllpLCD->frameDescriptorCh0fd1->FIDR  = LCD_FIDR(0);

	// Set the DMA transfer length to the size of the frame buffer
	pXllpLCD->frameDescriptorCh0fd1->LDCMD = LCD_Len(pXllpLCD->FrameBufferSize);

	// Store the physical address of this frame descriptor in the frame descriptor
	pXllpLCD->frameDescriptorCh0fd1->PHYSADDR = pXllpLCD->frameDescriptorCh0fd1->FDADR;

	// FBR0 is cleared and is not used.
	p_LCDRegs->FBR0 = 0;

	// Load the contents of FDADR0 with the physical address of this frame descriptor
	p_LCDRegs->FDADR0 = LCD_FDADR(pXllpLCD->frameDescriptorCh0fd1->FDADR);

	// Determine the LCLK frequency programmed into the CCCR.
	// This value will be used to calculate a Pixel Clock Divisor (PCD)
	// for a given display type.
	CCCR_L = (p_CLKRegs->cccr & 0x0000001F);

	if (CCCR_L < 8) // L = [2 - 7]
		LCLK = (13 * CCCR_L) * 100;
	else if (CCCR_L < 17) // L = [8 - 16]
		LCLK = ((13 * CCCR_L) * 100) >> 1;
	else if (CCCR_L < 32) // L = [17 - 31]
		LCLK = ((13 * CCCR_L) * 100) >> 2;

	// Convert the bpp setting into a value that the LCD controller understands.
	switch(pXllpLCD->BPP)
	{
		case BPP_1:
			BPP = 0;
			break;
		case BPP_2:
			BPP = 1;
			break;
		case BPP_4:
			BPP = 2;
			break;
		case BPP_8:
			BPP = 3;
			break;
		case BPP_16:
			BPP = 4;
			break;
		case BPP_18:
			BPP = 5;
			break;
		case BPP_18_PACKED:
			BPP = 6;
			break;
		case BPP_19:
			BPP = 7;
			break;
		case BPP_19_PACKED:
			BPP = 8;
			break;
		case BPP_24:
			BPP = 9;
			break;
		case BPP_25:
			BPP = 10;
			break;
		default:
 			BPP = 0;
			break;
 	}

	switch(pXllpLCD->DisplayType)
	{
	case TOSHIBAQVGARGB:
		{
			PCD = (LCLK / (2 * ToshibaQVGA_PIXEL_CLOCK_FREQUENCY));

			// Configure the LCD Controller Control Registers
			p_LCDRegs->LCCR0 = (LCD_LDM | LCD_SFM | LCD_IUM | LCD_EFM |
								LCD_PAS | LCD_QDM | LCD_BM  | LCD_OUM |
								LCD_RDSTM | LCD_CMDIM | LCD_LDDALT /*| LCD_OUC*/);

			p_LCDRegs->LCCR1 = (LCD_PPL(0xEF) | LCD_HSW(0x4) |
								LCD_ELW(0x0)  | LCD_BLW(0x1) );

			p_LCDRegs->LCCR2 = (LCD_LPP(0x13f) | LCD_VSW(0x0) |
								LCD_EFW(0xa)  | LCD_BFW(0x0a) );

			p_LCDRegs->LCCR3 = (LCD_PCD(PCD)  | LCD_BPP(BPP) | LCD_PCP |LCD_VSP | LCD_HSP |
								LCD_PDFOR(pXllpLCD->PixelDataFormat));

#if 0
			p_LCDRegs->LCCR4 = LCD_PAL_FOR(0);
			if ( (p_LCDRegs->OVL1C1 & LCD_O1EN) || (p_LCDRegs->OVL2C1 & LCD_O2EN))
			{
				p_LCDRegs->LCCR4 = LCD_PAL_FOR(2);
			}
#endif
            p_LCDRegs->LCCR4 = LCD_PAL_FOR(2);

			p_LCDRegs->LCCR4 |= LCD_K1(6) | LCD_K2(6) | LCD_K3(6);
		}
        	break;
      default:
 		break;
	}
}

void LCDClearStatusReg(P_XLLP_LCD_T pXllpLCD)
{
	LCDRegs *p_LCDRegs;
	p_LCDRegs = (LCDRegs *) pXllpLCD->LCDC;

	// Clear the status registers by writing 1's to each bit.
	p_LCDRegs->LCSR0 =	( LCD_LDD | LCD_SOF0| LCD_BER | LCD_ABC	| LCD_IU0	|
						  LCD_IU1 |	LCD_OU  | LCD_QD  | LCD_EOF0| LCD_BS0	|
						  LCD_SINT|	LCD_RD_ST | LCD_CMD_INTR );

	p_LCDRegs->LCSR1 =	( LCD_SOF1|	LCD_SOF2| LCD_SOF3| LCD_SOF4| LCD_SOF5	| LCD_SOF6	|
						  LCD_EOF1| LCD_EOF2| LCD_EOF3| LCD_EOF4| LCD_EOF5	| LCD_EOF6	|
						  LCD_BS1 | LCD_BS2	| LCD_BS3 |	LCD_BS4 | LCD_BS5	| LCD_BS6	|
									LCD_IU2 | LCD_IU3 | LCD_IU4 | LCD_IU5	| LCD_IU6 );

}

void KeypadInitLED(P_XLLP_LCD_T pXllpLCD)
{
    XLLP_GPIO_T *p_GPIORegs;
	p_GPIORegs = (XLLP_GPIO_T *) pXllpLCD->GPIO;
	p_GPIORegs->GPDR0 |= XLLP_GPIO_BIT_RDY;
    p_GPIORegs->GPCR0 |= XLLP_GPIO_BIT_RDY;
}

void KeypadOffLED(P_XLLP_LCD_T pXllpLCD)
{
    XLLP_GPIO_T *p_GPIORegs;
	p_GPIORegs = (XLLP_GPIO_T *) pXllpLCD->GPIO;
	p_GPIORegs->GPCR0 |= XLLP_GPIO_BIT_RDY;
}

void LCDSetupGPIOs(P_XLLP_LCD_T pXllpLCD)
{
	XLLP_GPIO_T *p_GPIORegs;

	p_GPIORegs = (XLLP_GPIO_T *) pXllpLCD->GPIO;

	p_GPIORegs->GPDR0 |= ( XLLP_GPIO_LCD_RSB | XLLP_GPIO_LCD_SCL |\
	                       XLLP_GPIO_LCD_CS | XLLP_GPIO_LCD_SI );
//	p_GPIORegs->GPSR0 |= XLLP_GPIO_BIT_PWM_OUT0;

	//p_GPIORegs->GAFR0_U |= (XLLP_GPIO_AF_BIT_PWM_OUT0);

	p_GPIORegs->GPDR1 |= ( XLLP_BIT_31 | XLLP_BIT_30 | XLLP_BIT_29 | \
	                       XLLP_BIT_28 | XLLP_BIT_27 | XLLP_BIT_26 );

	//p_GPIORegs->GPDR1 |= 0xfc000000;               // GPIO63 -- 58
	p_GPIORegs->GAFR1_U |= 0xaaa00000;
	p_GPIORegs->GPDR2 |= (XLLP_BIT_0 | XLLP_BIT_1 | XLLP_BIT_2 | XLLP_BIT_3 | XLLP_BIT_4 |
						    XLLP_BIT_5 | XLLP_BIT_6 | XLLP_BIT_7 | XLLP_BIT_8 | XLLP_BIT_9 |
						    XLLP_BIT_10 | XLLP_BIT_11 | XLLP_BIT_12 | XLLP_BIT_22 | XLLP_BIT_23);
	//p_GPIORegs->GPDR2 |= 0x00c01fff;               // GPIO 64 --  76   86-87
	p_GPIORegs->GAFR2_L |= 0x02aaaaaa;
	p_GPIORegs->GAFR2_U |= 0x0000a000;

	return;
}

void LCDEnableController(P_XLLP_LCD_T pXllpLCD)
{
	LCDRegs *p_LCDRegs;
	p_LCDRegs = (LCDRegs *) pXllpLCD->LCDC;

    p_LCDRegs->LCCR0 |= LCD_ENB;
}

void XllpLCD_DMALength(P_XLLP_OVERLAY_T pXllpOverlay)
{
	unsigned int pixels = pXllpOverlay->OverlayHeight * pXllpOverlay->OverlayWidth;
	unsigned int DMALength = 0;

	// Determine the DMA transfer length
	// each DMA transfer length for YUV formatted data must be multiples of 32-bits and adjusted accordingly
	if (pXllpOverlay->Format == FORMAT_RGB)
	{
		switch(pXllpOverlay->OverlayBPP)
		{
			case O_BPP_4:
				DMALength = pixels >> 1;
				break;
			case O_BPP_8:
				DMALength = pixels;
				break;
			case O_BPP_16:
				DMALength = pixels << 1;
				break;
			case O_BPP_18:
				DMALength = pixels << 2;
				break;
			case O_BPP_18_PACKED:
				break;
			case O_BPP_19:
				DMALength = pixels << 2;
				break;
			case O_BPP_19_PACKED:
			    DMALength = pixels * 3;
				break;
			case O_BPP_24:
				DMALength = pixels << 2;
				break;
			case O_BPP_25:
				DMALength = pixels << 2;
				break;
		default:
			break;
		}
		pXllpOverlay->ch2_size = DMALength;
		pXllpOverlay->ch3_size = 0;
		pXllpOverlay->ch4_size = 0;
	}
	if (pXllpOverlay->Format == FORMAT_PACKED_444)
	{
		pXllpOverlay->ch2_size = (pixels << 2);
		pXllpOverlay->ch3_size = 0;
		pXllpOverlay->ch4_size = 0;
	}
	else if (pXllpOverlay->Format == FORMAT_PLANAR_444)
	{
		// calculate the number of bits in the frame (pixels << 3)
		// mod by 32 to determine the remainder
		// subtract from 32 to determine how many bits to add to the length to make it a multiple of 32 bits
		// add this value to the number of bits in the frame
		// convert this value back to bytes
		DMALength = pixels;
		if ((DMALength % 4) > 0)
		{
			DMALength = (((32 - ((pixels << 3) % 32)) + (pixels << 3)) >> 3);  // 24 bits total
		}
		pXllpOverlay->ch2_size = DMALength;
		pXllpOverlay->ch3_size = DMALength;
		pXllpOverlay->ch4_size = DMALength;
	}
	else if (pXllpOverlay->Format == FORMAT_PLANAR_422)
	{
		DMALength = pixels;
		if ((DMALength % 4) > 0)
		{
			DMALength = (((32 - ((pixels << 3) % 32)) + (pixels << 3)) >> 3);	// 16 bits total
		}
		pXllpOverlay->ch2_size = DMALength;

		DMALength = pixels >> 1;
		if (((pixels << 2) % 32) > 0)
		{
			DMALength = (((32 - ((pixels << 2) % 32)) + (pixels << 2)) >> 3);
		}
		pXllpOverlay->ch3_size = DMALength;
		pXllpOverlay->ch4_size = DMALength;
	}
	else if (pXllpOverlay->Format == FORMAT_PLANAR_420)
	{
		DMALength = pixels;
		if ((DMALength % 4) > 0)
		{
			DMALength = (((32 - ((pixels << 3) % 32)) + (pixels << 3)) >> 3);	// 12 bits total
		}
		pXllpOverlay->ch2_size = DMALength;

		DMALength = pixels >> 2;
		if (((pixels << 1) % 32) > 0)
		{
			DMALength = (((32 - ((pixels << 1) % 32)) + (pixels << 1)) >> 3);
		}
		pXllpOverlay->ch3_size = DMALength;
		pXllpOverlay->ch4_size = DMALength;
	}
}

XLLP_STATUS_T XllpLCD_Overlay1_Enable (P_XLLP_LCD_T pXllpLCD, P_XLLP_OVERLAY_T pXllpOverlay)
{
    int bLoadPalette = 0;
    LCDRegs *p_LCDRegs;
//    unsigned int StatMask = 0;
    p_LCDRegs = (LCDRegs *) pXllpLCD->LCDC;

    if ((p_LCDRegs->OVL1C1) & LCD_O1EN)
	{
		kprintf("XllpLCD_Overlay1_Enable: Already enabled\r\n");
		return(0);
	}

	p_LCDRegs->LCCR0 |= LCD_OUC; // overlay
//	p_LCDRegs->LCCR0 &= ~LCD_OUC; // underlay

    XllpLCD_DMALength(pXllpOverlay);

    // Configure the general purpose frame descriptors
	// Set the physical address of the frame descriptor
	pXllpLCD->frameDescriptorCh1->FDADR = LCD_FDADR(pXllpLCD->_DMA_CHANNEL_1_FRAME_DESCRIPTOR_BASE_PHYSICAL);

	// Set the physical address of the frame buffer
	pXllpLCD->frameDescriptorCh1->FSADR = LCD_FSADR(pXllpLCD->_OVERLAY1_CHANNEL_BASE_PHYSICAL);

	// Clear the frame ID
	pXllpLCD->frameDescriptorCh1->FIDR  = LCD_FIDR(0);

	// Set the DMA transfer length to the size of the frame buffer
	pXllpLCD->frameDescriptorCh1->LDCMD = LCD_Len(pXllpOverlay->ch2_size); // Acturally ch1

	// Store the physical address of this frame descriptor in the frame descriptor
	pXllpLCD->frameDescriptorCh1->PHYSADDR = pXllpLCD->frameDescriptorCh1->FDADR;

	// frameDescriptorCh1fd2 is used only if a palette load is performed.
	// Set the physical address of the frame descriptor
	pXllpLCD->frameDescriptorCh1fd2->FDADR = LCD_FDADR(pXllpLCD->_DMA_CHANNEL_1_FRAME_DESCRIPTOR_BASE_PHYSICAL);

	// Set the physical address of the frame buffer
	pXllpLCD->frameDescriptorCh1fd2->FSADR = LCD_FSADR(pXllpLCD->_OVERLAY1_CHANNEL_BASE_PHYSICAL);

	// Clear the frame ID
	pXllpLCD->frameDescriptorCh1fd2->FIDR  = LCD_FIDR(0);

	// Set the DMA transfer length to the size of the frame buffer
	pXllpLCD->frameDescriptorCh1fd2->LDCMD = LCD_Len(pXllpOverlay->ch2_size); // Acturally ch1

	// Store the physical address of this frame descriptor in the frame descriptor
	pXllpLCD->frameDescriptorCh1fd2->PHYSADDR = pXllpLCD->frameDescriptorCh1fd2->FDADR;


    // FBR1 is cleared and is not used.
	p_LCDRegs->FBR1 = 0;

    pXllpOverlay->TmpBPP = pXllpLCD->BPP;
    pXllpLCD->BPP = BPP_19;

    p_LCDRegs->OVL1C2 = LCD_O1YPOS(pXllpOverlay->Y_Position) | LCD_O1XPOS(pXllpOverlay->X_Position);
	p_LCDRegs->OVL1C1 = (LCD_O1EN | LCD_BPP1(pXllpOverlay->OverlayBPP) | LCD_LPO1(pXllpOverlay->OverlayHeight-1) | LCD_PPL1(pXllpOverlay->OverlayWidth-1));

    // Wait for the hardware to signal input FIFOx underrun
	// before setting the FDADRx regs (and hence enabling DMA).
	// This is a key part of the hardware workaround described in Tibet sighting 49219.
	// Note that that IUx doesn't happen the first time after reset or resume.
	// The underrun apparently does occur in the controller but the controller doesn't set IUx.
	// According HW eng, the 50ms OSTDelay introduced by waiting for IUx should be sufficient
	// to avoid the hardware problem.
	// Note also that this may only needed for YUV modes but also works for RGB modes and
	// implementing it for all modes produces less convoluted code.
	D_Delay(50*1000);

	// Load the FDADRx regs with the physical address of the frame descriptors
	// Testing indicates that this is what actually enables DMA transfers.
	// If FDADRx isn't set (even if already set to the desired value) the DMA doesn't start.
	p_LCDRegs->FDADR1 = LCD_FDADR(pXllpLCD->frameDescriptorCh1->FDADR);

	// If required, load the default palette into palette ram
	// and feed this to the LCD controller.
	if (pXllpOverlay->OverlayBPP == 0x2 /* 4bits */) {
	    bLoadPalette = 1;
	    pXllpLCD->PaletteSize = 64;
	}

	if (pXllpOverlay->OverlayBPP == 0x3 /* 8bits */) {
	    bLoadPalette = 1;
	    pXllpLCD->PaletteSize = 1024;
	}

    if (bLoadPalette) XllpLCDLoadPalette(pXllpLCD);

    return 0;
}

void XllpLCD_Overlay1_Disable(P_XLLP_LCD_T pXllpLCD, P_XLLP_OVERLAY_T pXllpOverlay)
{
    volatile LCDRegs *p_LCDRegs;

	p_LCDRegs = (LCDRegs *) pXllpLCD->LCDC;

	if (!(p_LCDRegs->OVL1C1 & LCD_O1EN)) {
		// Not enable
		return;
	}

    p_LCDRegs->OVL1C1 &= 0x00FFFFFF; // Clear the enable bit, and clear the reserved bits 30:24.

    // Wait for DMA branch taken status (BSx bits to be set).
	// This is per HW Eng. workaround described in Tibet sighting 49219.
	// Note that testing indicates the loading of FBRx avoids the hardware problem.
	// Not loading FBRx and just waiting for 50ms does not avoid the problem.
	p_LCDRegs->FBR1 = LCD_FDADR(pXllpLCD->frameDescriptorCh1->FDADR) | (LCD_BRA | LCD_BINT);
    D_Delay(50 * 1000);
}

XLLP_STATUS_T XllpLCD_Overlay2_Enable (P_XLLP_LCD_T pXllpLCD, P_XLLP_OVERLAY_T pXllpOverlay)
{
    LCDRegs *p_LCDRegs;
    unsigned int StatMask = 0;
    p_LCDRegs = (LCDRegs *) pXllpLCD->LCDC;

    if ((p_LCDRegs->OVL2C1) & LCD_O2EN)
	{
		kprintf("XllpLCD_Overlay2_Enable: Already enabled\r\n");
		return(0);
	}

	p_LCDRegs->LCCR0 &= ~LCD_OUC;

    XllpLCD_DMALength(pXllpOverlay);

    // Configure the general purpose frame descriptors
	// Set the physical address of the frame descriptor
	pXllpLCD->frameDescriptorCh2_YCbCr_Y->FDADR = LCD_FDADR(pXllpLCD->_DMA_CHANNEL_2_Y_FRAME_DESCRIPTOR_BASE_PHYSICAL);
    pXllpLCD->frameDescriptorCh3_YCbCr_Cb->FDADR = LCD_FDADR(pXllpLCD->_DMA_CHANNEL_3_Cb_FRAME_DESCRIPTOR_BASE_PHYSICAL);
	pXllpLCD->frameDescriptorCh4_YCbCr_Cr->FDADR = LCD_FDADR(pXllpLCD->_DMA_CHANNEL_4_Cr_FRAME_DESCRIPTOR_BASE_PHYSICAL);

	// Set the physical address of the frame buffer
	pXllpLCD->frameDescriptorCh2_YCbCr_Y->FSADR = LCD_FSADR(pXllpLCD->_OVERLAY2_Y_CHANNEL_BASE_PHYSICAL);
    pXllpLCD->frameDescriptorCh3_YCbCr_Cb->FSADR = LCD_FSADR(pXllpLCD->_OVERLAY2_Cb_CHANNEL_BASE_PHYSICAL);
	pXllpLCD->frameDescriptorCh4_YCbCr_Cr->FSADR = LCD_FSADR(pXllpLCD->_OVERLAY2_Cr_CHANNEL_BASE_PHYSICAL);

	// Clear the frame ID
	pXllpLCD->frameDescriptorCh2_YCbCr_Y->FIDR  = LCD_FIDR(0);
    pXllpLCD->frameDescriptorCh3_YCbCr_Cb->FIDR  = LCD_FIDR(0);
	pXllpLCD->frameDescriptorCh4_YCbCr_Cr->FIDR  = LCD_FIDR(0);

	// Set the DMA transfer length to the size of the frame buffer
	pXllpLCD->frameDescriptorCh2_YCbCr_Y->LDCMD = LCD_Len(pXllpOverlay->ch2_size);
	pXllpLCD->frameDescriptorCh3_YCbCr_Cb->LDCMD = LCD_Len(pXllpOverlay->ch3_size);
	pXllpLCD->frameDescriptorCh4_YCbCr_Cr->LDCMD = LCD_Len(pXllpOverlay->ch4_size);

	// Store the physical address of this frame descriptor in the frame descriptor
	pXllpLCD->frameDescriptorCh2_YCbCr_Y->PHYSADDR = pXllpLCD->frameDescriptorCh2_YCbCr_Y->FDADR;
    pXllpLCD->frameDescriptorCh3_YCbCr_Cb->PHYSADDR = pXllpLCD->frameDescriptorCh3_YCbCr_Cb->FDADR;
	pXllpLCD->frameDescriptorCh4_YCbCr_Cr->PHYSADDR = pXllpLCD->frameDescriptorCh4_YCbCr_Cr->FDADR;

    // FBRx is cleared and is not used.
	p_LCDRegs->FBR2 = 0;
	p_LCDRegs->FBR3 = 0;
	p_LCDRegs->FBR4 = 0;

//    XllpLCDSuspend(pXllpLCD, Suspend_Graceful);

    pXllpOverlay->TmpBPP = pXllpLCD->BPP;
    pXllpLCD->BPP = BPP_19;

    StatMask = LCD_IU2;
	if (pXllpOverlay->Format != FORMAT_RGB)
		StatMask |= (LCD_IU3 | LCD_IU4);
	p_LCDRegs->LCSR1 = StatMask; // clear

    p_LCDRegs->OVL2C2 = (LCD_FOR(pXllpOverlay->Format) | LCD_O2YPOS(pXllpOverlay->Y_Position) | LCD_O2XPOS(pXllpOverlay->X_Position));
	p_LCDRegs->OVL2C1 = (LCD_O2EN | LCD_BPP2(pXllpOverlay->OverlayBPP) | LCD_LPO2(pXllpOverlay->OverlayHeight-1) | LCD_PPL2(pXllpOverlay->OverlayWidth-1));

    // Wait for the hardware to signal input FIFOx underrun
	// before setting the FDADRx regs (and hence enabling DMA).
	// This is a key part of the hardware workaround described in Tibet sighting 49219.
	// Note that that IUx doesn't happen the first time after reset or resume.
	// The underrun apparently does occur in the controller but the controller doesn't set IUx.
	// According HW eng, the 50ms OSTDelay introduced by waiting for IUx should be sufficient
	// to avoid the hardware problem.
	// Note also that this may only needed for YUV modes but also works for RGB modes and
	// implementing it for all modes produces less convoluted code.
	D_Delay(50*1000);


//	if (WaitForState(&(p_LCDRegs->LCSR1),StatMask,WFS_ALL,50) < 0)
//		kprintf("XllpLCD_Overlay2_Enable(F=%d): No IUx (LCSR1=x%08x)\r\n",
//		pXllpOverlay->Format, p_LCDRegs->LCSR1);
//	else
//		kprintf("XllpLCD_Overlay2_Enable(F=%d): Got IUx (LCSR1=x%08x)\r\n",
//		pXllpOverlay->Format, p_LCDRegs->LCSR1);

	// Load the FDADRx regs with the physical address of the frame descriptors
	// Testing indicates that this is what actually enables DMA transfers.
	// If FDADRx isn't set (even if already set to the desired value) the DMA doesn't start.
	p_LCDRegs->FDADR2 = LCD_FDADR(pXllpLCD->frameDescriptorCh2_YCbCr_Y->FDADR);
	if (pXllpOverlay->Format != FORMAT_RGB)
	{
		p_LCDRegs->FDADR3 = LCD_FDADR(pXllpLCD->frameDescriptorCh3_YCbCr_Cb->FDADR);
	    p_LCDRegs->FDADR4 = LCD_FDADR(pXllpLCD->frameDescriptorCh4_YCbCr_Cr->FDADR);
	}

	p_LCDRegs->LCSR1 = StatMask; // clear

//    XllpLCDResume(pXllpLCD);

    return 0;
}

void XllpLCD_Overlay2_Disable(P_XLLP_LCD_T pXllpLCD, P_XLLP_OVERLAY_T pXllpOverlay)
{
    volatile LCDRegs *p_LCDRegs;
    unsigned int OVL2C1;
    unsigned int OVL2C2;
    unsigned int OL2Format;
	unsigned int StatMask = 0;

	p_LCDRegs = (LCDRegs *) pXllpLCD->LCDC;

	OVL2C1 = p_LCDRegs->OVL2C1;
	if (!(OVL2C1 & LCD_O2EN)) {
		// Not enable
		return;
	}

    OVL2C2 = p_LCDRegs->OVL2C2;
    OL2Format = (OVL2C2 >> 20) & 7;
	StatMask = LCD_BS2;
	if (OL2Format != FORMAT_RGB)
		StatMask |= (LCD_BS3 | LCD_BS4);
	p_LCDRegs->LCSR1 = StatMask;

    p_LCDRegs->OVL2C1 &= 0x00FFFFFF; // Clear the enable bit, and clear the reserved bits 30:24.

	// Reinit the LCD controller so that the pixel data format can be updated for use without overlays
//	XllpLCDSuspend(pXllpLCD, Suspend_Graceful);

//	pXllpLCD->PixelDataFormat = PDFOR_11;

	// Undo the degraded base frame - always.
//	pXllpLCD->BPP = pXllpOverlay->TmpBPP;

    // Wait for DMA branch taken status (BSx bits to be set).
	// This is per HW Eng. workaround described in Tibet sighting 49219.
	// Note that testing indicates the loading of FBRx avoids the hardware problem.
	// Not loading FBRx and just waiting for 50ms does not avoid the problem.
	p_LCDRegs->FBR2 = LCD_FDADR(pXllpLCD->frameDescriptorCh2_YCbCr_Y->FDADR) | (LCD_BRA | LCD_BINT);
	if (OL2Format != FORMAT_RGB)
	{
		p_LCDRegs->FBR3 = LCD_FDADR(pXllpLCD->frameDescriptorCh3_YCbCr_Cb->FDADR) | (LCD_BRA | LCD_BINT);
		p_LCDRegs->FBR4 = LCD_FDADR(pXllpLCD->frameDescriptorCh4_YCbCr_Cr->FDADR) | (LCD_BRA | LCD_BINT);
	}

    D_Delay(50 * 1000);

//	XllpLCDResume(pXllpLCD);
}
